#ifndef _KALMAN_FILTER_H_
#define _KALMAN_FILTER_H_

#include <Eigen/Dense>

class KalmanFilter
{

public:
    KalmanFilter(int state_dim, int meas_dim);
    ~KalmanFilter();

private:
    int state_dim;
    int meas_dim;
    Eigen::VectorXd x; // 状态估计
    Eigen::MatrixXd P; // 状态协方差矩阵
    Eigen::MatrixXd Q; // 过程噪声协方差矩阵
    Eigen::MatrixXd R; // 观测噪声协方差矩阵
    Eigen::MatrixXd F; // 状态转移矩阵
    Eigen::MatrixXd H; // 观察矩阵
    Eigen::MatrixXd B; // 控制矩阵

public:
    void KalmanFilter::init(const Eigen::VectorXd &x0, const Eigen::MatrixXd &P0,
                            const Eigen::MatrixXd &Q_in, const Eigen::MatrixXd &R_in,
                            const Eigen::MatrixXd &F_in, const Eigen::MatrixXd &H_in);

    void predict(const Eigen::VectorXd &u = Eigen::VectorXd::Zero(0));
    void update(const Eigen::VectorXd &z);

    Eigen::VectorXd getState() const;
};
#endif